#include "Arm.h"
#include "Roller.h"
#include "Auton.h"
#include "WPILib.h"

class Team830Robot : public SimpleRobot
{
	// Driver Station Input
	static const UINT32 JOYSTICKL = 1;
	static const UINT32 JOYSTICKR = 2;

	// PWM Channels
	static const UINT32 DRIVE_LEFT1 = 1;
	static const UINT32 DRIVE_LEFT2 = 2;
	static const UINT32 DRIVE_RIGHT1 = 3;
	static const UINT32 DRIVE_RIGHT2 = 4;
	static const UINT32 ROLLER_VIC_TOP = 6;
	static const UINT32 ROLLER_VIC_BOT = 7;
	static const UINT32 ARM_VIC = 8;
	static const UINT32 SERVO_MINI = 9;

	// Digital Ports
	static const UINT32 PHOTO_L = 1;
	static const UINT32 PHOTO_M = 2;
	static const UINT32 PHOTO_R = 3;
	static const UINT32 PRESSURE_SWITCH = 6;
	static const UINT32 ENCODER_L_A = 7;
	static const UINT32 ENCODER_L_B = 8;
	static const UINT32 ENCODER_R_A = 9;
	static const UINT32 ENCODER_R_B = 10;
	//static const UINT32 SERVO_MINI = 11;
	
	// Relay Ports
	static const UINT32 COMPRESSOR = 5;
	
	// Analog Channels
	static const UINT32 GYRO = 1;
	static const UINT32 ARM_POT = 2;
	
	// Solenoid Channels
	static const UINT32 MINIBOT_SOLE = 1;
	static const UINT32 WRIST_SOLE_IN = 2;
	static const UINT32 WRIST_SOLE_OUT = 3; //we have 10
	
	// Named conversions from button numbers for
	// the Logitech Dual-Action Controller
	enum JoypadButton { FACE_W = 1, FACE_S = 2, FACE_E = 3, FACE_N = 4, 
						BUMPER_L1 = 5, BUMPER_R1 = 6, BUMPER_L2 = 7, BUMPER_R2 = 8, 
						AUX_L = 9, AUX_R = 10 };
	
	enum JoypadAxis { STICK_LX = 1, STICK_LY = 2,
					  STICK_RX = 3, STICK_RY = 4,
					  DPAD_X = 5, DPAD_Y = 6 };
	
	//SetR is for control of the asole; mini is for minibot deploy; feeder is feeder height
	enum CBControls { ROLL_IN = 1, ROLL_OUT = 3, SLOW_IN = 4, SLOW_OUT = 2, 
					  RACK_HIGH = 6, RACK_MID = 7, RACK_LOW = 0, MNT_MINI = 10, 
					  WRIST_OUT = 9, WRIST_IN = 8, ARM_MINI = 11 };
	
	// A function to alter the linear input from the analog sticks
	// to something less twitchy.
	float AccelFunc(float input)
	{
		// Dead zone
		float dz = 0.1;
		
		if (input < dz && input > -dz) input = 0;
		else input = input > 0 ? (input - dz) / (1 - dz) : (input + dz) / (1 - dz);
		
		// Hard check to make sure we don't pass outside
		// the range -1 to 1 to the Victors.
		return (input > 1) ? 1 : ((input < -1) ? -1 : input);
	}
	
	RobotDrive*		drive;
	Arm*			arm;
	Roller*			roller;
	Auton*			auton;
	
	Joystick*		pilot;
	Joystick*		copilot;
	
	Compressor*		comp;
	Solenoid*		minibotSole;
	
	Timer*          timer;
	Servo*          servo;
	Gyro*			gyro;
	
	// These will fire continuously, providing power to 
	// the light sensors.
	Solenoid*		lightSole_l;
	Solenoid*		lightSole_m;
	Solenoid*		lightSole_r;
	
public:
	Team830Robot() {
		// The victors are created explicitly and then passed
		// to the RobotDrive object, because the RobotDrive constructor
		// expects Jaguars by default.
		drive 	= new RobotDrive(new Victor(DRIVE_LEFT1),
								 new Victor(DRIVE_LEFT2),
								 new Victor(DRIVE_RIGHT1),
								 new Victor(DRIVE_RIGHT2));
		
		arm 	= new Arm(ARM_VIC, ARM_POT, WRIST_SOLE_IN, WRIST_SOLE_OUT);
		roller 	= new Roller(ROLLER_VIC_TOP, ROLLER_VIC_BOT);
		auton	= new Auton(drive, arm, roller, ENCODER_L_A, ENCODER_L_B, ENCODER_R_A, ENCODER_R_B, gyro);
		
		pilot 	= new Joystick(JOYSTICKL);
		copilot = new Joystick(JOYSTICKR);
		
		timer   = new Timer();
		gyro    = new Gyro(GYRO);
		servo   = new Servo(SERVO_MINI);
		
		/*
		printf("Constructing camera.\n");
		AxisCamera& cam = AxisCamera::GetInstance();
		
		printf("Setting camera parameters.\n");
		cam.WriteResolution(AxisCamera::kResolution_320x240);
		cam.WriteMaxFPS(24);
		cam.WriteCompression(20);
		cam.WriteBrightness(0);
		*/
		
		printf("Constructing compressor.\n");
		comp = new Compressor(PRESSURE_SWITCH, COMPRESSOR);
		comp->Start();
		
		printf("Constructing solenoids.\n");
		minibotSole = new Solenoid(MINIBOT_SOLE);
		lightSole_l = new Solenoid(6);
		lightSole_m = new Solenoid(7);
		lightSole_r = new Solenoid(8);
	}
	
	~Team830Robot()
	{ 
		delete drive;
		delete arm;
		delete roller;
		delete auton;
		
		delete pilot;
		delete copilot;
		delete timer;
		
		delete comp;
		delete minibotSole;
		delete lightSole_l;
		delete lightSole_m;
		delete lightSole_r;
		
		
	}
	
	////////////////////////////// Loop Routines //////////////////////////////
	void Disabled(void)
	{
		drive->TankDrive(0.0, 0.0);
		drive->Drive(0.0, 0.0);
		Wait(0.005);
	}
	
	
	void Autonomous(void)
	{
		auton->Start();
		
		/*
		printf("Powering solenoids.\n");
		lightSole_l->Set(true);
		lightSole_m->Set(true);
		lightSole_r->Set(true);
		*/
		
	}
	
	void OperatorControl(void)
	{
		while (IsOperatorControl()) {
			
			/////////////////
			// Pilot Input //
			/////////////////
			
			float l, r;
			
			if (pilot->GetRawAxis(DPAD_Y) > 0) {
				l = 0.6;
				r = 0.6;
			}
			else if (pilot->GetRawAxis(DPAD_Y) < 0) {
				l = -0.6;
				r = -0.6;
			}
			else {
				// Arcade Drive
				if (pilot->GetRawButton(BUMPER_R1)) {
					float speed    = AccelFunc(pilot->GetRawAxis(STICK_LY));
					float turnrate = AccelFunc(pilot->GetRawAxis(STICK_RX)) * 2;
					
					l = turnrate > 0 ? 1 : 1 - turnrate;
					r = turnrate < 0 ? 1 : 1 - turnrate;
					
					l *= speed;
					r *= speed;
				}
				
				// Tank Drive
				else {
					l = AccelFunc(pilot->GetRawAxis(STICK_LY));
					r = AccelFunc(pilot->GetRawAxis(STICK_RY));
				}
				
				// Slow Mode
				if (pilot->GetRawButton(BUMPER_L1)) {
					l *= 0.8;
					r *= 0.8;
				}
			}
			
			drive->TankDrive(l, r);
			
			// If the pilot presses both auxillary buttons at the same
			// time, raise the minibot up.
			if (pilot->GetRawButton(7) && pilot->GetRawButton(8))  {
				minibotSole->Set(true);
			}
			if (pilot->GetRawButton(9) && pilot->GetRawButton(10))  {
				minibotSole->Set(false);
			}
			
			///////////////////
			// Copilot Input //
			///////////////////
			
			float armSpeed = -AccelFunc(copilot->GetRawAxis(2));
			
			/*if (copilot->GetRawButton(RACK_LOW)) {
				arm->MoveTo(Arm::LOW);
			} else if (copilot->GetRawButton(RACK_MID)) {
				arm->MoveTo(Arm::MID);
			} else if (copilot->GetRawButton(RACK_HIGH)) {
				arm->MoveTo(Arm::HIGH);
			} else if (copilot->GetRawButton(SLOT)) {
				arm->MoveTo(Arm::SLOT);
			} */
			if(1) {
				arm->Rotate(armSpeed * 0.6);
			}
			
			if (copilot->GetRawButton(ROLL_IN)) {
				roller->Spin(1.0);
			}
			else if (copilot->GetRawButton(SLOW_IN)) {
				roller->Spin(0.8);
			}
			else if (copilot->GetRawButton(SLOW_OUT)) {
				roller->Spin(-0.8);
			}
			else if (copilot->GetRawButton(ROLL_OUT)){
				roller->Spin(-1.0);
			}
			else {
				roller->Spin(0);
			}
			
			if (copilot->GetRawButton(WRIST_OUT)) {
				arm->ToggleArm(true);
			} else if (copilot->GetRawButton(WRIST_IN)) {
				arm->ToggleArm(false);
			}
			if (copilot->GetRawButton(ARM_MINI)) {
				servo->SetAngle(180);
			}
			if (copilot->GetRawButton(MNT_MINI)) {
				servo->SetAngle(0);
			}
			
			//////////////////
			// End of Input //
			//////////////////
			
			Wait(0.005);
		}
	}
};

START_ROBOT_CLASS(Team830Robot);
